from sls_controller.sls_uavResource import UAVResource
from sls_controller.sls_ugvResource import UGVResource
from sls_controller.sls_publicDefine import *
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy

from libs.sls_mirrors.sls_agent import MirrorsAgent

import math
import json
import os
from datetime import datetime
import clibs.azimuthalProjection as azimuthalProjection
from geopy.distance import geodesic
from geographiclib.geodesic import Geodesic

class CrossResource():
    _instance = None
    _initialized = False
    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super().__new__(cls)
        return cls._instance
    def __init__(self):
        if not self._initialized:
            #### USER CODE BEGIN ####
            self.this = "this"
            self.objects = 123
            self.Is = ["abc",123]
            self.a_test = {"ads":123,456:"adsd"}
            #### USER CODE END ####
            self._initialized = True
    def to_dict(self):
        return {
            key: value
            for key, value in self.__dict__.items()
            if not key.startswith("_") and not callable(value)
        }
    def to_json(self):
        return json.dumps(self.to_dict())
class ROSResource():
    _instance = None
    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super().__new__(cls)
        return cls._instance
    
    def __init__(self, main_node: Node):
        self.main_node = main_node
        self.qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )        
    # publisher subscription manager
    ## publisher   
    ### create_publisher
    def create_publisher(
        self,
        msg_type,
        topic: str,
        qos_profile = None,
        *,
        callback_group = None,
        event_callbacks = None,
        qos_overriding_options = None
    ):
        if qos_profile is None:
            qos_profile = self.qos_profile
        return self.main_node.create_publisher(
            msg_type,
            topic,
            qos_profile,
            callback_group=callback_group,
            event_callbacks=event_callbacks,
            qos_overriding_options=qos_overriding_options)
    
    ###delete_publisher
    def destroy_publisher(self, publisher):
        self.main_node.destroy_publisher(publisher)

    def get_logger(self):
        return self.main_node.get_logger()

    ## subscription
    ### create_subscription
    def create_subscription(
        self,
        msg_type,
        topic: str,
        callback,
        qos_profile = None,
        *,
        callback_group = None,
        event_callbacks = None,
        qos_overriding_options = None,
        raw: bool = False
    ):
        if qos_profile is None:
            qos_profile = self.qos_profile        
        return self.main_node.create_subscription(
            msg_type,
            topic,
            callback,
            qos_profile,
            callback_group=callback_group,
            event_callbacks=event_callbacks,
            qos_overriding_options=qos_overriding_options,
            raw=raw
        ) 
    ### destroy_subscription
    def destroy_subscription(self, subscription):
        self.main_node.destroy_subscription(subscription)
    def create_client(self, srv_type, srv_name):
        return self.main_node.create_client(srv_type, srv_name)
    def destroy_client(self, client):
        return self.main_node.destroy_client(client)


class ToolsResource():
    _instance = None
    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super().__new__(cls)
        return cls._instance
    
    def __init__(self):
        self.file_path = None
    
    def azimuthal_projection(self,latitude:float,longitude:float,ref_latitude:float,ref_longitude:float):
        return azimuthalProjection.project(latitude,longitude,ref_latitude,ref_longitude)
    
    def claculate_distanceAndcourseAngle(self,orig_x:float,orig_y:float,dest_x:float,dest_y:float)->tuple[float,float]:
        initial_angle = math.atan2(dest_y-orig_y,dest_x-orig_x)
        if initial_angle < 0:
            initial_angle += 2*math.pi
        return math.dist([orig_x,orig_y],[dest_x,dest_y]),initial_angle
    
    def calculate_gps_distance(self, gps1: tuple, gps2: tuple) -> float:
        """
        计算两个GPS坐标点之间的球面距离
        
        参数:
            gps1: 第一个GPS坐标 (lat1, lon1) 单位：度
            gps2: 第二个GPS坐标 (lat2, lon2) 单位：度
        例子：
            calculate_gps_distance((32.196888,119.510418),(32.197079,119.509061))
        返回:
            distance: 两点之间的距离，单位：米
        """
        return geodesic(gps1, gps2).meters
    
    def calculate_cross_track(self,local_gps: tuple, target_gps: tuple) -> float:
        """
        计算在ENU坐标系下，以第一个点为参考点，第二个点的横向偏移（东方向距离）
        
        参数:
            local_gps: 参考点GPS坐标 (lon, lat) 单位：度
            target_gps: 目标点GPS坐标 (lon, lat) 单位：度
        
        返回:
            cross_track: 横向偏移量，单位：米（东方向为正，西方向为负）
        """
        # 解包坐标（经度, 纬度）
        lon1, lat1 = local_gps
        lon2, lat2 = target_gps
        
        # 将角度转换为弧度
        lat1_rad = math.radians(lat1)
        lon1_rad = math.radians(lon1)
        lat2_rad = math.radians(lat2)
        lon2_rad = math.radians(lon2)
        
        # 地球半径（米）
        R = 6371000
        
        # 计算经度方向的距离（东方向）
        # 使用中间纬度来近似计算经度方向的距离
        avg_lat_rad = (lat1_rad + lat2_rad) / 2
        dlon = lon2_rad - lon1_rad
        
        # 东方向距离 = 经度差 × 该纬度的经度每度对应的距离
        cross_track = dlon * R * math.cos(avg_lat_rad)
        
        return cross_track
    
    def normalize_angle_180(self, angle: float) -> float:
        """归一化角度到 (-180, 180]"""
        return (angle + 180) % 360-180

    def normalize_angle_pi(self, rad: float) -> float:
        """归一化弧度到 (-π, π]"""
        rad %= (2 * math.pi)
        return (rad + math.pi) % (2 * math.pi)-math.pi
    def swap_NED_ENU_yaw(self,yaw: float) -> float:
        """
        实现 NED 和 ENU 坐标系之间的航向角互转。
        参数:
            yaw: 原始航向角，单位度（°），范围不限。
        返回:
            float: 转换后的航向角，范围 [0, 360)°。
        """
        return (450 - yaw) % 360

    def gps_to_local_NED(self, current_gps,target_gps):
        """
        将目标 GPS 坐标转换为当前 GPS 坐标的 NED（北-东-下）坐标系

        !!!!!!!!!!!! yaw是ENU的yaw!!!!!!!!!!!! 
        
        参数:
            current_gps: 参考点 (lat, lon)，单位：度
            target_gps: 目标点 (lat, lon)，单位：度
        
        返回:
            (east, north, azimuth)
                - east: 东向距离（米）
                - north: 北向距离（米）
                - yaw: 当前点指向目标点的方位角（度，0°为正北，顺时针增加）
        """
        (cur_lat, cur_lon) = current_gps
        (tgt_lat, tgt_lon) = target_gps
        geod = Geodesic.WGS84
        g = geod.Inverse(cur_lat, cur_lon, tgt_lat, tgt_lon)
        azimuth = g['azi1']
        distance = g['s12']
        east = distance * math.sin(math.radians(azimuth))
        north = distance * math.cos(math.radians(azimuth))
        yaw = math.atan2(north, east)*180/3.14159265359
        return (east, north, yaw)
    
    def set_save_path_name(self,equipment_unit = "uv",filename="ugv_name"):
            timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
            filename = f"{timestamp}.txt"
            if not os.path.exists("point/"+equipment_unit):
                os.makedirs("point/"+equipment_unit)
            self.file_path = os.path.join("point/"+equipment_unit, filename)
        
    def save_data(self, lat, lon, lat_err, yaw_err, steer):
        """
        保存数据到TXT文件,每次运行都创建新文件
        
        参数:
        lat: 当前纬度
        lon: 当前经度  
        lat_err: 当前横偏
        yam_err: 当前航偏
        steer: 当前转角
        """
        try:
            if self.file_path is not None:
                with open(self.file_path , 'a', encoding='utf-8') as file:
                    data_line = f"{lat},{lon},{lat_err},{yaw_err},{steer}\n"
                    file.write(data_line)
                return self.file_path
        except Exception as e:
            file.close()
            print(e)
            return None

class PublicResource():
    #sigular instance
    _instance = None
    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super().__new__(cls)
        return cls._instance
    
    def __init__(self, ros_resource: ROSResource,mirrors_agent:MirrorsAgent, uav_resource: UAVResource = None, ugv_resource: UGVResource = None):
        self.ros_resource = ros_resource
        self.mirrors_agent = mirrors_agent
        self.uav_resource = uav_resource
        self.ugv_resource = ugv_resource
        if self.uav_resource is not None:
            self.vehicle_class = VehicleClass.UAV
        elif self.ugv_resource is not None:
            self.vehicle_class = VehicleClass.UGV
        self.tools_resource = ToolsResource()
        self.cross_resource = CrossResource()
    
    #get the timer during of main timer
    def get_main_delta_time(self) -> float:
        return self.ros_resource.main_node.main_timer_during

if __name__ == "__main__":
    toolsResource = ToolsResource()
    print(toolsResource.azimuthal_projection(47.3978,8.5461,47.0,8.0))
    print(toolsResource.azimuthal_projection(47.3978,8.5461,47.3978,8.5461))
    print(toolsResource.claculate_distanceAndcourseAngle(0,0,0,0.0))
    crossResource = CrossResource()
    print(crossResource.to_dict())